function op = ke(t,x,param)
% This one is for a double pendulum

q(1:3,1) = x(1:3);
u(:,1) = x(7:9);
m(1) = param{1};
La = param{2};
Iner(:,:,1) = param{3};

q(1:3,2) = x(4:6);
u(:,2) = x(10:12);
m(2) = param{4};
Lb = param{5};
Iner(:,:,2) = param{6};

% -------------------------------------------------------------
for i = 1:2
    c1(i) = cos(q(1,i));
    c2(i) = cos(q(2,i));
    c3(i) = cos(q(3,i));
    s1(i) = sin(q(1,i));
    s2(i) = sin(q(2,i));
    s3(i) = sin(q(3,i));
end

% -------------------------------------------------------------
% Transformation matrices

for i = 1:2
    pr_P_K(1:3,1:3,i) = [c2(i)*c3(i)                   -c2(i)*s3(i)                    s2(i);
                        s1(i)*s2(i)*c3(i)+s3(i)*c1(i) -s1(i)*s2(i)*s3(i)+c1(i)*c3(i) -s1(i)*c2(i);
                        -c1(i)*s2(i)*c3(i)+s3(i)*s1(i) c1(i)*s2(i)*s3(i)+c3(i)*s1(i)  c1(i)*c2(i);];
end

N_C_K(1:3,1:3,1) = pr_P_K(1:3,1:3,1);
N_C_K(1:3,1:3,2) = N_C_K(1:3,1:3,1)*pr_P_K(1:3,1:3,2);

% Kinematical differential equations
for i = 1:2    
    W_Kde_qdot(1:3,1:3,i) = [ c2(i)*c3(i) s3(i) 0;
                             -c2(i)*s3(i) c3(i) 0;
                              s2(i)       0     1;];
    Wb(1:3,i) = W_Kde_qdot(1:3,1:3,i)*u(:,i);
end

% Convert Angular velocities to Newtonian Basis and frame.
WN(1:3,1) = N_C_K(:,:,1)*Wb(:,1);
WN(1:3,2) = WN(:,1) + N_C_K(:,:,2)*Wb(:,2);

% -------------------------------------------------------------
% Handles in Body Basis

cm2H1(:,1) = [0 0 La/2]';
cm2H2(:,1) = [0 0 -La/2]';

cm2H1(:,2) = [0 0 Lb/2]';
cm2H2(:,2) = [0 0 -Lb/2]';

% -------------------------------------------------------------
% Convert to Newtonian basis
for i = 1:2
    cmtoH1(:,i) = N_C_K(:,:,i)*cm2H1(:,i);
    cmtoH2(:,i) = N_C_K(:,:,i)*cm2H2(:,i);
    H2toH1(:,i) = -cmtoH2(:,i) + cmtoH1(:,i);
    
    I(:,:,i) = N_C_K(:,:,i)*Iner(:,:,i)*(N_C_K(:,:,i)'); % This step is useless for the values of inertia selected  
    ke_w(i) = 0.5*WN(:,i)'*I(:,:,i)*WN(:,i);
end

vcm(:,1) = cross(WN(:,1),-cmtoH2(:,1));
va(:,1) =  cross(WN(:,1),H2toH1(:,1));
vcm(:,2) = va(:,1) + cross(WN(:,2),-cmtoH2(:,2));

ke_v = 0.5*m(1)*dot(vcm(:,1),vcm(:,1)) + 0.5*m(2)*dot(vcm(:,2),vcm(:,2));
KE = ke_v + ke_w(1) + ke_w(2);
cmz1 = -cmtoH2(2,1);
cmz2 = -2*cmtoH2(2,1) - cmtoH2(2,2);
PE =(m(1)*cmz1 + m(2)*cmz2)*9.81;
E =  KE+PE;
op = E;
% keyboard
% -------------------------------------------------------------